//
// Created by haoy on 2017/6/20.
//

#include "rtp/rtp_fec.hpp"

#include "xcutil.h"
#include "xm_log.h"

int FecHeader::read(const uint8_t* data, const int size) {
    if (size < 8) {
        return 0;
    }
    e = data[0] >> 7;
    l = data[0] >> 6 & 0x1;
    p = data[0] >> 5 & 0x1;
    x = data[0] >> 4 & 0x1;
    cc = data[0] & 0xF;
    m = data[1] >> 7;
    pt = data[1] & 0x7F;
    seq = be_get_u16(data + 2);
    ts = be_get_u32(data + 4);
    len = be_get_u16(data + 8);
    return 10;
}

int FecHeader::write(uint8_t* data, const int size) const {
    if (size < 10) {
        return 0;
    }
    data[0] = e << 7 | l << 6 | p << 5 | x << 4 | cc;
    data[1] = m << 7 | pt;
    be_set_u16(seq, data + 2);
    be_set_u32(ts, data + 4);
    be_set_u16(len, data + 8);
    return 10;
}

int FecLevelHeader::read(const uint8_t* data, const int size, uint8_t l) {
    if ((l && size < 8) || size < 4) {
        return 0;
    }
    protection_len = be_get_u16(data);
    mask = be_get_u16(data + 2);
    mask = mask << 48;
    if (l == 1) {
        uint64_t mask_cont = be_get_u32(data + 4);
        mask |= mask_cont << 16;
    }
    return l == 1 ? 8 : 4;
}

int FecLevelHeader::write(uint8_t* data, const int size, uint8_t l) const {
    if ((l && size < 8) || size < 4) {
        return 0;
    }
    be_set_u16(protection_len, data);
    uint16_t m1 = mask >> 48;
    be_set_u16(m1, data + 2);
    if (l == 1) {
        uint32_t m2 = mask >> 16 & 0xFFFFFFFF;
        be_set_u32(m2, data + 4);
    }
    return l == 1 ? 8 : 4;
}

int RtpFec::read(const uint8_t* data, const int size) {
    if (size < 14) {
        return 0;
    }

    level_header.protected_seqs.clear();
    int hdr_size = header.read(data, size);
    if (hdr_size == 0) {
        return 0;
    }

    auto lhsize = level_header.read(data + hdr_size, size - hdr_size, header.l);
    if (lhsize == 0) {
        return 0;
    }

    payload = const_cast<uint8_t*>(data + hdr_size + lhsize);
    payload_size = size - hdr_size - lhsize;

    uint64_t mask = level_header.mask;
    uint16_t seq = header.seq;
    while (mask) {
        uint8_t msb = mask >> 63;
        if (msb == 1) {
            level_header.protected_seqs.emplace_back(seq);
        }
        mask = mask << 1;
        ++seq;
    }

    if (level_header.protected_seqs.empty()) {
        return 0;
    }
    return size;
}

int RtpFec::write(uint8_t* data, const int size) const {
    int header_size = 10;
    int level_header_size = 2 + header.l ? 2 : 6;

    if (size < header_size + level_header_size + payload_size) {
        return 0;
    }

    int hdr_size = header.write(data, size);
    if (hdr_size == 0) {
        return 0;
    }

    int lvl_hdr_size = level_header.write(data + hdr_size, size - hdr_size, header.l);

    memcpy(data + hdr_size + lvl_hdr_size, payload, payload_size);

    return header_size + level_header_size + payload_size;
}